//------------------------------------------------------------------------------
//  <copyright file="GroundTruthLogger.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;

    /// <summary>
    /// Ground Truth Logger class
    /// </summary>
    [DataContract]
    public class GroundTruthLogger : IDisposable
    {
        /// <summary>
        /// Name of the landmark-relative ground truth file (used by navigation benchmark). This lists poses relative to the currently observed landmark.
        /// </summary>
        private const string LandmarkRelativeGroundTruthFilename = "groundTruthLandmarkRelative.xml";

        /// <summary>
        /// Name of the absolute ground truth file. This has poses in "global" coordinates.  
        /// </summary>
        private const string AbsoluteGroundTruthFilename = "groundTruthAbsolute.txt";

        /// <summary>
        /// Name of the ground truth in external (relative) format.  This has delta x, y, heading relative to the previous pose.
        /// The format of this file is in accordance with external data sets like those from Freiburg.
        /// </summary>
        private const string ExternalGroundTruthFilename = "groundTruthExternal.txt";

        /// <summary>
        /// Path to the output directory
        /// </summary>
        private readonly string outputDir;

        /// <summary>
        /// Path to the landmarks list file.  This is the list of landmarks that will be visited.
        /// </summary>
        private readonly string landmarkListFile;

        /// <summary>
        /// The landmarks ids we will visit
        /// </summary>
        private List<int> landmarkIds;

        /// <summary>
        /// Which waypoint are we at
        /// </summary>
        private int waypointIndex;

        /// <summary>
        /// The start pose for this ground truth segment
        /// </summary>
        private Pose2D startPose;

        /// <summary>
        /// Stream writer for the external format output
        /// </summary>
        private StreamWriter externalFormatwriter;

        /// <summary>
        /// Stream writer for the absolute format output
        /// </summary>
        private StreamWriter absoluteFormatwriter;

        /// <summary>
        /// list of ground truth poses relative to the landmarks
        /// </summary>
        private List<Pose2D> landmarkRelativeGroundTruthPoses;

        /// <summary>
        /// Initializes a new instance of the <see cref="GroundTruthLogger"/> class.
        /// </summary>
        /// <param name="outputDir">The path to the output file</param>
        /// <param name="landmarksListFile">File containing the ids of the landmarks we will visit</param>
        public GroundTruthLogger(string outputDir, string landmarksListFile)
        {
            this.outputDir = outputDir;
            this.landmarkListFile = landmarksListFile;

            this.Initialize();
            Console.WriteLine("Ready to start recording ground truth file. Press left shoulder button once at each waypoint and wait for confirmation of pose gathering.");
        }

        /// <summary>
        /// Gets the current landmark ID
        /// </summary>
        public int CurrentLandmarkId
        {
            get
            {
                return this.landmarkIds[Math.Min(this.waypointIndex, this.landmarkIds.Count - 1)];
            }
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="GroundTruthLogger"/> class.
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
        }

        /// <summary>
        /// Takes care of closing files.
        /// </summary>
        public void Close()
        {
            if (this.externalFormatwriter != null)
            {
                this.externalFormatwriter.Close();
            }

            if (this.absoluteFormatwriter != null)
            {
                this.absoluteFormatwriter.Close();
            }
        }

        /// <summary>
        /// Log the current pose in the external dataset format
        /// </summary>
        /// <param name="globalPose">The current global pose</param>
        /// <param name="localPose">The current local pose</param>
        /// <returns>Boolean indicating if the landmark was captured. False means there were no more landmarks in the list</returns>
        public bool LogGroundTruthPose(Pose2D globalPose, Pose2D localPose)
        {
            if (this.waypointIndex >= this.landmarkIds.Count)
            {
                Console.WriteLine("{0} Landmarks captured... no more in the list", this.landmarkIds.Count);
                return false;
            }

            this.absoluteFormatwriter.WriteLine("{0} {1:F3} {2:F3} {3:F3}", this.landmarkIds[this.waypointIndex], globalPose.X, globalPose.Y, globalPose.Heading);

            this.landmarkRelativeGroundTruthPoses.Add(localPose);
            DataLoader<List<Pose2D>>.ExportData(Path.Combine(this.outputDir, LandmarkRelativeGroundTruthFilename), this.landmarkRelativeGroundTruthPoses);

            if (this.waypointIndex == 0)
            {
                // this is the first one. Initialize;
                this.startPose = globalPose;
            }
            else
            {
                double dx = globalPose.X - this.startPose.X;
                double dy = globalPose.Y - this.startPose.Y;

                // project dx and dy into the frame of the start pose.
                double length = Math.Sqrt((dx * dx) + (dy * dy));
                double angle = Math.Atan2(dy, dx) - this.startPose.Heading;
                double newX = length * Math.Cos(angle);
                double newY = length * Math.Sin(angle);

                double theta = Pose2D.ClampAngle(globalPose.Heading - this.startPose.Heading);

                this.externalFormatwriter.WriteLine("{0} {1} {2:F3} {3:F3} 0.000 0.000 0.000 {4:F3}", this.waypointIndex - 1, this.waypointIndex, newX, newY, theta);
                
                // remember the previous pose for next time
                this.startPose = globalPose;
            }

            Console.WriteLine("     Captured pose {0} of {1} at LandmarkId:{2}", this.waypointIndex + 1, this.landmarkIds.Count, this.landmarkIds[this.waypointIndex]);
            this.waypointIndex++;
            return true;
        }

        /// <summary>
        /// Initialization of class members
        /// </summary>
        private void Initialize()
        {
            this.externalFormatwriter = new StreamWriter(Path.Combine(this.outputDir, ExternalGroundTruthFilename));
            this.externalFormatwriter.AutoFlush = true;
            this.externalFormatwriter.WriteLine("#start 0.0");

            this.absoluteFormatwriter = new StreamWriter(Path.Combine(this.outputDir, AbsoluteGroundTruthFilename));
            this.absoluteFormatwriter.AutoFlush = true;

            this.landmarkRelativeGroundTruthPoses = new List<Pose2D>();

            this.landmarkIds = new List<int>();

            // loop over all entries in the waypoint file and add them to our list of waypoints.
            using (StreamReader fileReader = new StreamReader(this.landmarkListFile))
            {
                while (!fileReader.EndOfStream)
                {
                    string line = fileReader.ReadLine();
                    if (string.IsNullOrEmpty(line) || line[0] == '#')
                    {
                        continue;
                    }

                    string[] data = line.Split(new[] { ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
                    this.landmarkIds.Add(int.Parse(data[0]));
                }
            }
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="GroundTruthLogger"/> class.
        /// </summary>
        /// <param name="disposing">True if disposing, false if finalizing.</param>
        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                this.Close();

                GC.SuppressFinalize(this);
            }
        }
    }
}
